% 验证误差协方差矩阵更新算法

clc;
clear;
load('mcsim', 'in');
par.g = 9.7934752114984;
cst = load('earthconstants_WGS84.mat', 'RE');
cst.deg2Rad = pi / 180;
cst.sec2Rad = pi / 3600 / 180;
in.curSimParVal(1:36, 1) = in.parRefVal(1:36, 2);

%% 生成轨迹
% 常量
cst_trajGen = load('earthconstants_WGS84.mat', 'omegaIE', 'RE', 'epsilonSq');
% 配置
cfg_trajGen.enableTrajectoryGen = true;
cfg_trajGen.enableSensErr = true; % 在轨迹中引入器件误差
cfg_trajGen.enableSensRandErr = false;
cfg_trajGen.enableNavigation = false;
cfg_trajGen.ignore2OdSensErr = true;
cfg_trajGen.IMUOutFilePath = [];
cfg_trajGen.quantizeIMUOut = false;
cfg_trajGen.IMUOutPrecision = '%.16e';
% 参量
par_trajGen.samplePeriod = 0.01;
par_trajGen.g = par.g; % 重力加速度，m/s^2
par_trajGen.IMU.acc.B = in.curSimParVal(10:12, 1) * par_trajGen.g * 1e-6; % 转换前单位μg，转换后单位m/s^2
par_trajGen.IMU.acc.SF0 = ones(3, 1);
par_trajGen.IMU.acc.dSF = in.curSimParVal(13:15, 1) * 1e-6;
par_trajGen.IMU.acc.MA = vec2offdiag(in.curSimParVal(16:21, 1)) * 1e-6; % 转换前单位μrad，转换后单位rad
par_trajGen.IMU.acc.SO = in.curSimParVal(22:24, 1) * 1e-6 / par_trajGen.g; % 转换前单位μg/g^2，转换后单位1/(m/s^2)
par_trajGen.IMU.gyro.B = in.curSimParVal(25:27, 1) * cst.sec2Rad; % 转换前单位°/h，转换后单位rad/s
par_trajGen.IMU.gyro.SF0 = ones(3, 1);
par_trajGen.IMU.gyro.dSFp = in.curSimParVal(28:30, 1) * 1e-6;
par_trajGen.IMU.gyro.dSFn = par_trajGen.IMU.gyro.dSFp;
par_trajGen.IMU.gyro.MA = vec2offdiag(in.curSimParVal(31:36, 1)) * 1e-6; % 转换前单位μrad，转换后单位rad
par_trajGen.IMU.gyro.GS = zeros(3);
% 输入量
in_trajGen.genFuncName = 'gentgi_navtest_regular';
in_trajGen.genFuncInArg = {};
% 初始条件
iniCon_trajGen.CBG = eye(3); % 初始姿态矩阵
iniCon_trajGen.vG = zeros(3, 1); % 初始速度
iniCon_trajGen.pos = [30 * cst.deg2Rad, 110 * cst.deg2Rad, 0]'; % 纬度、经度、高度
% 生成轨迹
[out_trajGen, ~] = gentrajectory(in_trajGen, cfg_trajGen, par_trajGen, iniCon_trajGen, cst_trajGen);

%% 运行组合导航
% 常量
cst_intNav = load('earthconstants_WGS84.mat', 'RE', 'e', 'mu', 'J2', 'J3', 'omegaIE', 'epsilonSq');
% 配置
cfg_intNav.enableKalmFilt = false;
cfg_intNav.enableKFUpdate = false;
cfg_intNav.extrapAlgo = KalmFiltExtrapAlgo.ITER;
cfg_intNav.updAlgo = KalmFiltUpdAlgo.NORMAL;
cfg_intNav.useDCMInAttCalc = false;
cfg_intNav.dynaMatINSSysBlkForm = 1;
cfg_intNav.navCoordType = NavCoordType.FREE_AZM;
cfg_intNav.sysStateMask = [true(1, 10) false(1, 1)];
cfg_intNav.sensStateMask = [true(1, 27) false(1, 9)];
cfg_intNav.enableNumCondCtrl = false; % 关闭数值条件控制
% 参量
par_intNav.tBgn = out_trajGen.t(1);
par_intNav.tEnd = out_trajGen.t(end);
par_intNav.dT = par_trajGen.samplePeriod;
par_intNav.tUpd = 20; % Aid组合导航的量测更新周期
par_intNav.g = par_trajGen.g;
par_intNav.INSCycPerKalFiltExtrapCyc = int32(50);
par_intNav.GammaMRGammaMT = diag([0.001/cst.RE, 0.001/cst.RE, 0.001, 0.001, 0.001, 0.001 1e-6 1e-6 1e-6 0.001/cst.RE].^2);
par_intNav.INSSensRWC = eps*ones(6, 1); % par_intNav.INSSensRWC及par_intNav.INSSensStateProcNoisePSDFull取0会导致in_kalmFilt.GPQGPT矩阵所有元素为0，进而导致out_kalmFilt.P对角线元素出现负数，故取eps而不是0
par_intNav.INSSensStateProcNoisePSDFull = eps*ones(36, 1);
par_intNav.PDiagMin = NaN(nnz([cfg_intNav.sysStateMask cfg_intNav.sensStateMask]), 1);
par_intNav.PDiagMax = NaN(nnz([cfg_intNav.sysStateMask cfg_intNav.sensStateMask]), 1);
% 初始条件
iniCon_intNav.pos = out_trajGen.pos(1, :)'; % 无误差
iniCon_intNav.vN = out_trajGen.vG(1, :)'; % 无误差
iniCon_intNav.CBN = out_trajGen.CBG(:, :, 1); % 无误差
iniCon_intNav.kalmFilt.x = [-in.curSimParVal(1)/cst.RE; in.curSimParVal(2)/cst.RE; in.curSimParVal(3); in.curSimParVal(6:-1:4); in.curSimParVal(9:-1:7)*cst.sec2Rad; in.curSimParVal(2)/cst.RE*tan(out_trajGen.pos(1, 1));...
    in.curSimParVal(10:12)*par_intNav.g*1e-6; in.curSimParVal(13:21)*1e-6; in.curSimParVal(22:24)/par_intNav.g*1e-6; in.curSimParVal(25:27)*cst.sec2Rad; in.curSimParVal(28:36)*1e-6];
iniCon_intNav.kalmFilt.P = iniCon_intNav.kalmFilt.x*iniCon_intNav.kalmFilt.x';
% 输入量
m = size(out_trajGen.accOutErrFree, 1);
in_intNav.INS.accOut = out_trajGen.accOutErrFree;
in_intNav.INS.gyroOut = out_trajGen.gyroOutErrFree;
in_intNav.aid.CNE = zeros(3, 3, m);
in_intNav.aid.vN = zeros(m, 3);
in_intNav.aid.vG = out_trajGen.vG;
in_intNav.aid.CBN = zeros(3, 3, m);
in_intNav.aid.h = out_trajGen.pos(:, 3);
in_intNav.aid.alpha = zeros(m, 1);
in_intNav.traj.CNE = zeros(3, 3, m);
in_intNav.traj.pos = out_trajGen.pos;
in_intNav.traj.vN = out_trajGen.vG;
in_intNav.traj.CBN = zeros(3, 3, m);
in_intNav.traj.fB = out_trajGen.fBTrue;
in_intNav.traj.omegaIBB = out_trajGen.omegaIBBTrue;
in_intNav.traj.FCN = zeros(3, 3, m);
in_intNav.traj.gN = zeros(m, 3);
in_intNav.u = NaN(0, 1);
% 运行导航
clear('intnav_kfvld');
% 纯惯导解算(TODO: 因为轨迹发生器无vN, CNE, CBN, FCN, gN, alpha参数输出，故利用纯惯导解算获取这些参数，后续待轨迹发生器完善之后可删除此段纯惯导解算代码)
[out_inertNavIdeal, ~] = intnav_kfvld(in_intNav, cfg_intNav, par_intNav, iniCon_intNav, cst_intNav);
% 组合导航
cfg_intNav.enableKalmFilt = true;
cfg_intNav.enableKFUpdate = true;
iniCon_intNav.pos = out_trajGen.pos(1, :)' + [in.curSimParVal(1, 1)/cst.RE; in.curSimParVal(2, 1)/cst.RE*sec(out_trajGen.pos(1, 1)); in.curSimParVal(3)];
iniCon_intNav.vN = out_trajGen.vG(1, :)' + [in.curSimParVal(6, 1); in.curSimParVal(5, 1); in.curSimParVal(4, 1)];
iniCon_intNav.CBN = angle2dcm(in.curSimParVal(7)*cst.sec2Rad, in.curSimParVal(8)*cst.sec2Rad, in.curSimParVal(9)*cst.sec2Rad) * out_trajGen.CBG(:, :, 1);
in_intNav.INS.accOut = out_trajGen.accOut;
in_intNav.INS.gyroOut = out_trajGen.gyroOut;
in_intNav.aid.CNE = out_inertNavIdeal.CNE;
in_intNav.aid.vN = out_inertNavIdeal.vN;
in_intNav.aid.CBN = out_inertNavIdeal.CBN;
in_intNav.aid.alpha = out_inertNavIdeal.alpha;
in_intNav.traj.CNE = out_inertNavIdeal.CNE;
in_intNav.traj.CBN = out_inertNavIdeal.CBN;
in_intNav.traj.fB = out_trajGen.fBTrue;
in_intNav.traj.omegaIBB = out_trajGen.omegaIBBTrue;
in_intNav.traj.FCN = out_inertNavIdeal.FCN;
in_intNav.traj.gN = out_inertNavIdeal.gN;
[out_intNav_normal, auxOut_intNav_normal] = intnav_kfvld(in_intNav, cfg_intNav, par_intNav, iniCon_intNav, cst_intNav);
% 组合导航
cfg_intNav.updAlgo = KalmFiltUpdAlgo.JOSEPH;
[out_intNav_Joseph, auxOut_intNav_Joseph] = intnav_kfvld(in_intNav, cfg_intNav, par_intNav, iniCon_intNav, cst_intNav);

%% 后处理
[p, q] = size(out_intNav_normal.kalmFilt_x);
PErr_Max = NaN(p, 1);
PRelatErr_Max = NaN(p, 1);
PErr = auxOut_intNav_normal.kalmFilt_P - auxOut_intNav_Joseph.kalmFilt_P;
PRelatErr = relatdiff(auxOut_intNav_normal.kalmFilt_P, auxOut_intNav_Joseph.kalmFilt_P);
for i=1:p
    PErr_Max(i, 1) = max(max(abs(PErr(:, :, i))));
    PRelatErr_Max(i, 1) = max(max(abs(PRelatErr(:, :, i))));
end
preparefig('误差协方差矩阵更新算法误差');
subplot(2, 1, 1)
plot(out_intNav_normal.kalmFilt_tExtrap, PErr_Max);
title('P阵各元素的绝对误差的最大值');
xlabel('时间t');
subplot(2, 1, 2)
plot(out_intNav_normal.kalmFilt_tExtrap, PRelatErr_Max);
title('P阵各元素的相对误差的最大值');
xlabel('时间t');
ylabel('无单位');